package nav;

import tools.Timer2;
import lejos.util.TimerListener;

/**Odometer uses an internal timer to poll the FactoryFloorRobot and get displacement and heading
 * uses them to compute current position
 * @author Antoine Lennartz*/
public class Odometer implements TimerListener {
	/**default timer period*/
	public static final int DEFAULT_PERIOD = 15;
	/**link to the robot*/
	private TwoWheeledRobot robot;
	/**internal polling timer*/
	private Timer2 odometerTimer;
	/**lock object for position data*/
	private Object lock;
	/**position data x and y in cm, theta in rad*/
	private double x, y, theta;
	/**holder for previous tachoCount value*/
	private double leftTacho=0, rightTacho=0;
	
	/**Constructor
	 * @param robot - two wheeled Robot it belongs to
	 * @param period -  tacho count polling period
	 * @param start - immediate start of polling*/
	public Odometer(TwoWheeledRobot robot, int period, boolean start) {
		// initialise variables
		this.robot = robot;
		odometerTimer = new Timer2(period, this);
		x = 0.0;
		y = 0.0;
		theta = 0.0;
		lock = new Object();
		
		// start the odometer immediately, if necessary
		if (start)
			odometerTimer.start();
	}
	
	public Odometer(TwoWheeledRobot robot) {
		this(robot, DEFAULT_PERIOD, true);
	}
	
	public Odometer(TwoWheeledRobot robot, boolean start) {
		this(robot, DEFAULT_PERIOD, start);
	}
	
	public Odometer(TwoWheeledRobot robot, int period) {
		this(robot, period, true);
	}
	
	/**Polls tacho count and updates position and heading*/
	public void timedOut() {
		double dLeftTacho, dRightTacho, dPos, dTheta;
		dLeftTacho	=	robot.getLeftTacho()*Math.PI/180-leftTacho;
		dRightTacho	=	robot.getRightTacho()*Math.PI/180-rightTacho;
		dPos		=	(dRightTacho*robot.RIGHT_RADIUS+dLeftTacho*robot.LEFT_RADIUS)/2;
		dTheta		=	(-dLeftTacho*robot.LEFT_RADIUS+dRightTacho*robot.RIGHT_RADIUS)/robot.WIDTH;
		
		// update the position
		synchronized (lock) {
			x += dPos * Math.sin(theta + dTheta / 2.0);
			y += dPos * Math.cos(theta + dTheta / 2.0);
			theta += dTheta;
			
			// keep theta between 0 and 2*Math.PI
			if (theta < 0)
				theta += 2*Math.PI;
			else if (theta > 2*Math.PI)
				theta -= 2*Math.PI;
		}
		leftTacho+=dLeftTacho;
		rightTacho+=dRightTacho;
	}
	

	/**position of the robot
	 * @param pos - buffer to put position into assumes length=3*/
	public void getPosition(double [] pos) {
		synchronized (lock) {
			pos[0] = x;
			pos[1] = y;
			pos[2] = theta;
		}
	}
	/**@return the associated TwoWheeledRobot object */
	public TwoWheeledRobot getTwoWheeledRobot() {
		return robot;
	}
	
	
	/**for watch true value in the update array the value in pos overwrites the 
	 * corresponding position value
	 * @param pos - new position
	 * @param update - boolean array for updating only certain parts of position*/
	public void setPosition(double [] pos, boolean [] update) {
		synchronized (lock) {
			if (update[0]) x = pos[0];
			if (update[1]) y = pos[1];
			if (update[2]) theta = pos[2];
		}
	}
	
	/**for watch true value in the update array the value in corr is added to the 
	 * corresponding position value
	 * @param corr - corrections array
	 * @param update - boolean array for updating only certain parts of position*/
	public void corrPosition(double [] corr, boolean [] update) {
		synchronized (lock) {
			if (update[0]) x += corr[0];
			if (update[1]) y += corr[1];
			if (update[2]) theta += corr[2];
		}
	}
	
	
	
	
}
